/****************************************************************
 *
 * IMU Project 
 * 
 ****************************************************************/

/****************************************************************
 *
 *	Module		: "source file" myIMU.c
 *  Tool		: KeilC ARM 4.22a
 *	Chip		: STM32F407VG
 * 	History		: 04/08/2012
 *	Description	: 
				  + My library to communicate with IMU
				  + Reference to header file "myIMU.h"
 *				
 *	Author		: Dang Anh Tung			
 *	Description	: www.payitforward.edu.vn
 *				
 *
 ****************************************************************/

  /****************************************************************
 * 							IMPORT
 ****************************************************************/
 #include "myIMU.h"


 /****************************************************************
 * 							GLOBAL
 ****************************************************************/



/*****************************************************************
* 							DEFINE
******************************************************************/


 /****************************************************************
 * 						   FUNCTIONS
 ****************************************************************/

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    None
** Returned value:		None
** 
*****************************************************************/
uint32_t myIMUInit(void)
{
    uint32_t res = 0x00;
    if (myAccInit() == TRUE) { res |= ACC_SENSOR_OK;}
    if (myGyrInit() == TRUE) { res |= GYR_SENSOR_OK;}
    if (myMagInit() == TRUE) { res |= MAG_SENSOR_OK;}
    return res;

}

/****************************************************************
** Function name:	    
**
** Descriptions:        See "SensorsNote.docx"		
**
** parameters:		    Enable 3 axis x,y,z; 4g scale
** Returned value:		None
** 
*****************************************************************/
BOOL myAccInit(void)
{

#ifdef _USE_LSM330_BOARD_
    uint8_t ConfigAddr[7]  = {0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x2E};
    uint8_t ConfigValue[7] = {0x17, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00};
    myI2CWriteBytes(I2C_SOURCE, &ConfigValue[0], I2C_ACC_ADDR, ConfigAddr[0], 6);    
    myI2CWriteBytes(I2C_SOURCE, &ConfigValue[6], I2C_ACC_ADDR, ConfigAddr[6], 1);
    return TRUE;
#endif /* _USE_LSM330_BOARD_ */  
  
}

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    Enable 3 axis x,y,z; High-pass filter 50Hz
**                      500dps scale;
** Returned value:		None
** 
*****************************************************************/
BOOL myGyrInit(void)
{

#ifdef _USE_LSM330_BOARD_
    uint8_t ConfigAddr[6]  = {0x20, 0x21, 0x22, 0x23, 0x24, 0x2E};
    uint8_t ConfigValue[6] = {0xAF, 0x00, 0x00, 0x10, 0x13, 0x00};
    myI2CWriteBytes(I2C_SOURCE, &ConfigValue[0], I2C_GYR_ADDR, ConfigAddr[0], 5);    
    myI2CWriteBytes(I2C_SOURCE, &ConfigValue[5], I2C_GYR_ADDR, ConfigAddr[5], 1);
    return TRUE;
#endif /* _USE_LSM330_BOARD_ */
    
}

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    Single measurement mode, Max output rate 160Hz
**                      1.3Ga scale;
** Returned value:		None
** 
*****************************************************************/
BOOL myMagInit(void)
{

#ifdef _USE_LSM330_BOARD_
    uint8_t ConfigAddr[3]  = {0x00, 0x01, 0x02};
    uint8_t ConfigValue[3] = {0x78, 0x20, 0x01};
    myI2CWriteBytes(I2C_SOURCE, &ConfigValue[0], I2C_MAG_ADDR, ConfigAddr[0], 3);    
    return TRUE;
#endif /* _USE_LSM330_BOARD_ */
    
}

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    None
** Returned value:		None
** 
*****************************************************************/
BOOL myIMURead(INT_IMU *imu)
{
    BOOL res = TRUE;
    if (myAccRead(&(imu->acc)) == FALSE) { res = FALSE;}
    if (myGyrRead(&(imu->gyr)) == FALSE) { res = FALSE;}
    if (myMagRead(&(imu->mag)) == FALSE) { res = FALSE;}
    return res;
}

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    None
** Returned value:		None
** 
*****************************************************************/
BOOL myAccRead(INT_3D  *acc)
{

#ifdef _USE_LSM330_BOARD_
    uint8_t  readBuffer[7];
    
    myI2CReadBytes(I2C_SOURCE, &readBuffer[0], I2C_ACC_ADDR, I2C_ACC_STATUS_REG, 7);

    /* 
    * check bit 3: new data is available 
    * check bit 7: new data is overwritten (reading rate is lower than writing rate)
    */
    if ( (readBuffer[0] & 0x08) == 0x08 )  
    {
        /* New set data is available */
        acc->state = TRUE;
        acc->x = myTwoComp2Int(&readBuffer[2],&readBuffer[1]);
        acc->y = myTwoComp2Int(&readBuffer[4],&readBuffer[3]);
        acc->z = myTwoComp2Int(&readBuffer[6],&readBuffer[5]);
    }
    else
    {
        acc->state = FALSE;
    }
    return TRUE;
#endif /* _USE_LSM330_BOARD_ */  
      
}

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    None
** Returned value:		None
** 
*****************************************************************/
BOOL myGyrRead(INT_3D  *gyr)
{

#ifdef _USE_LSM330_BOARD_
    uint8_t  readBuffer[7];

    myI2CReadBytes(I2C_SOURCE, &readBuffer[0], I2C_GYR_ADDR, I2C_GYR_STATUS_REG, 7);

    /* 
    * check bit 3: new data is available 
    * check bit 7: new data is overwritten (reading rate is lower than writing rate)
    */
    if ( (readBuffer[0] & 0x08) == 0x08 )
    {
        /* New set data is available */
        gyr->state = TRUE;
        gyr->x = myTwoComp2Int(&readBuffer[2],&readBuffer[1]);
        gyr->y = myTwoComp2Int(&readBuffer[4],&readBuffer[3]);
        gyr->z = myTwoComp2Int(&readBuffer[6],&readBuffer[5]);
    }
    else
    {
        gyr->state = FALSE;
    }
    return TRUE;
#endif /* _USE_LSM330_BOARD_ */

}

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    None
** Returned value:		None
** 
*****************************************************************/
BOOL myMagRead(INT_3D  *mag)
{

#ifdef _USE_LSM330_BOARD_
    uint8_t  readBuffer[7];

    myI2CReadBytes(I2C_SOURCE, &readBuffer[0], I2C_ACC_ADDR, I2C_MAG_OUT_X_H, 7);

    /* 
    * check bit 0: new data is available 
    */
    if ( (readBuffer[6] & 0x01) == 0x01 )
    {
        /* New set data is available */
        mag->state = TRUE;
        mag->x = myTwoComp2Int(&readBuffer[0],&readBuffer[1]);
        mag->y = myTwoComp2Int(&readBuffer[2],&readBuffer[3]);
        mag->z = myTwoComp2Int(&readBuffer[4],&readBuffer[5]);
    }
    else
    {
        mag->state = FALSE;
    }
    return TRUE;
#endif /* _USE_LSM330_BOARD_ */    

}

/****************************************************************
** Function name:	    
**
** Descriptions:		
**
** parameters:		    - Accelerometer in m/s^2, Gyro in mdps, Mag in mGa
**                      -
** Returned value:		None
** 
*****************************************************************/
BOOL myInt2Real(CONVERT_TYPE type, INT_3D *i, REAL_3D *r)
{
    
#ifdef _USE_LSM330_BOARD_
    BOOL res = TRUE;
    switch (type)
    {
        case ACC_DATA_CONVERT:
            r->x = (REAL)(i->x * K_ACC_X);
            r->y = (REAL)(i->y * K_ACC_Y);
            r->z = (REAL)(i->z * K_ACC_Z);
            break;
        case GYR_DATA_CONVERT:
            r->x = (REAL)(i->x * K_GYR_X);
            r->y = (REAL)(i->y * K_GYR_Y);
            r->z = (REAL)(i->z * K_GYR_Z);
            break;
        case MAG_DATA_CONVERT:
            r->x = (REAL)(i->x * K_MAG_X);      
            r->y = (REAL)(i->y * K_MAG_Y);
            r->z = (REAL)(i->z * K_MAG_Z);
            break;
        default:
            res = FALSE;
            break;
    }
    return res;
#endif /* _USE_LSM330_BOARD_ */   

}

/****************************************************************
** Function name:   Convert Two's (2's) complement 16bit|MSB|LSB| to integer number   
**
** Descriptions:		
**
** parameters:		    None
** Returned value:		None
** 
*****************************************************************/
int16_t myTwoComp2Int(uint8_t *MSB, uint8_t *LSB)
{
    /* Algorithm 1 */
    int16_t val;
    uint8_t *p = (uint8_t *)(&val);
    *p++ = *LSB;
    *p = *MSB;
    return val;

//    /* Algorithm 2 */
//    int16_t res = 0;
//    res = (int16_t) ((*MSB)*256 + (*LSB));
//    if ((*MSB) & 0x80)
//    {
//        //negative number
//        res = res - 65536;
//        return res;
//    }
//    else
//    {
//        //positive number
//    }
//    return res;    
}

void myReal2Bytes(REAL r, uint8_t * byte)
{
    uint32_t i;
    uint8_t *p;
    p = (uint8_t*)&r;   /* casting a pointer to uint8_t */
    for ( i = 0; i <= 3; byte[3-i] = *p++, i++);
}


void myBytes2Real(REAL *r, uint8_t * byte)
{
    uint32_t i;
    uint8_t *p;    
    p = (uint8_t*)r; /* casting a pointer to uint8_t */
    for ( i = 0; i <= 3; *p++ = byte[3-i], i++);
}
 
/****************************************************************
** Function name:   Calculate 3 angle by raw data
**
** Descriptions:		
**
** parameters:		    None
** Returned value:		None
** 
*****************************************************************/

void myTestIMU(void)
{
    INT_IMU imu;
    REAL_3D a,g,m;
    REAL phi, theta, psi;
    if ( myIMURead(&imu) == TRUE)
    {
        myInt2Real(ACC_DATA_CONVERT, &imu.acc, &a);
        myInt2Real(GYR_DATA_CONVERT, &imu.gyr, &g);
        myInt2Real(MAG_DATA_CONVERT, &imu.mag, &m);
        phi = asin (a.y/sqrt(a.y * a.y + a.z * a.z));
        theta = - asin(a.x);        /* a.x/g but a.x is divided by 9.8 already */
        psi = -atan2(m.y, m.x);     /* when phi, theta approximate zero */
    }
}
